// PID automated tuning (Ziegler-Nichols/relay method) for Arduino and compatible boards
// Copyright (c) 2016-2020 jackw01
// This code is distrubuted under the MIT License, see LICENSE for details

#include "pid_autotune_zn.h"
#include <stdlib.h>
#include <stdint.h>
#include <stdio.h>
pid_autotune_zn_t *pid_autotune_zn_create()
{
  pid_autotune_zn_t *handle = (pid_autotune_zn_t *)malloc(sizeof(pid_autotune_zn_t));
  if (NULL == handle)
  {
    return NULL;
  }
  handle->targetInputValue = 0;
  handle->loopInterval = 0;
  handle->znMode = ZNModeNoOvershoot;
  handle->cycles = 10;
  return handle;
}

void pid_autotune_zn_free(pid_autotune_zn_t *handle)
{
  if (NULL != handle)
  {
    free(handle);
  }
}

void pat_zn_set_target_input_value(pid_autotune_zn_t *handle, float target)
{
  handle->targetInputValue = target;
}

void pat_zn_set_loop_interval(pid_autotune_zn_t *handle, long interval)
{
  handle->loopInterval =interval;
}

void pat_zn_set_output_range(pid_autotune_zn_t *handle, float min, float max)
{
  handle->minOutput = min;
  handle->maxOutput = max;
}

void pat_zn_set_ZNMode(pid_autotune_zn_t *handle, ZNMode zn)
{
  handle->znMode = zn;
}

void pat_zn_set_tuning_cycles(pid_autotune_zn_t *handle, int tuneCycles)
{
  handle->cycles = tuneCycles;
}

// Must be called immediately before the tuning loop starts
void pat_zn_start_tuning_loop(pid_autotune_zn_t *handle, unsigned long us)
{
  handle->i = 0;         // Cycle counter
  handle->output = true; // Current output state
  handle->outputValue = handle->maxOutput;
  handle->t1 = handle->t2 = us;                    // Times used for calculating period
 handle-> microseconds = handle->tHigh = handle->tLow = 0; // More time variables
  handle->max = -1000000000000;            // Max input
  handle->min = 1000000000000;             // Min input
  handle->pAverage = handle->iAverage = handle->dAverage = 0;
}

// Automatically tune PID
// This function must be run in a loop at the same speed as the PID loop being tuned
// See README for more details - https://github.com/jackw01/arduino-pid-autotuner/blob/master/README.md
float pat_zn_tune_PID(pid_autotune_zn_t *handle, float input, unsigned long us)
{
  // Useful information on the algorithm used (Ziegler-Nichols method/Relay method)
  // http://www.processcontrolstuff.net/wp-content/uploads/2015/02/relay_autot-2.pdf
  // https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method
  // https://www.cds.caltech.edu/~murray/courses/cds101/fa04/caltech/am04_ch8-3nov04.pdf

  // Basic explanation of how this works:
  //  * Turn on the output of the PID controller to full power
  //  * Wait for the output of the system being tuned to reach the target input value
  //      and then turn the controller output off
  //  * Wait for the output of the system being tuned to decrease below the target input
  //      value and turn the controller output back on
  //  * Do this a lot
  //  * Calculate the ultimate gain using the amplitude of the controller output and
  //      system output
  //  * Use this and the period of oscillation to calculate PID gains using the
  //      Ziegler-Nichols method

  // Calculate time delta
  // long prevMicroseconds = microseconds;
  handle->microseconds = us;
  // float deltaT = microseconds - prevMicroseconds;

  // Calculate max and min
  handle->max = (handle->max > input) ? handle->max : input;
  handle->min = (handle->min < input) ? handle->min : input;

  // Output is on and input signal has risen to target
  if (handle->output && input > handle->targetInputValue)
  {
    // Turn output off, record current time as t1, calculate tHigh, and reset maximum
   handle-> output = false;
    handle->outputValue = handle->minOutput;
    handle->t1 = us;
    handle->tHigh = handle->t1 - handle->t2;
    handle->max = handle->targetInputValue;
  }

  // Output is off and input signal has dropped to target
  if (!handle->output && input < handle->targetInputValue)
  {
    // Turn output on, record current time as t2, calculate tLow
    handle->output = true;
    handle->outputValue = handle->maxOutput;
    handle->t2 = us;
    handle->tLow = handle->t2 - handle->t1;

    // Calculate Ku (ultimate gain)
    // Formula given is Ku = 4d / πa
    // d is the amplitude of the output signal
    // a is the amplitude of the input signal
    float ku = (4.0 * ((handle->maxOutput - handle->minOutput) / 2.0)) / (M_PI * (handle->max - handle->min) / 2.0);

    // Calculate Tu (period of output oscillations)
    float tu = handle->tLow + handle->tHigh;

    // How gains are calculated
    // PID control algorithm needs Kp, Ki, and Kd
    // Ziegler-Nichols tuning method gives Kp, Ti, and Td
    //
    // Kp = 0.6Ku = Kc
    // Ti = 0.5Tu = Kc/Ki
    // Td = 0.125Tu = Kd/Kc
    //
    // Solving these equations for Kp, Ki, and Kd gives this:
    //
    // Kp = 0.6Ku
    // Ki = Kp / (0.5Tu)
    // Kd = 0.125 * Kp * Tu

    // Constants
    // https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method

    float kpConstant, tiConstant, tdConstant;
    if (handle->znMode == ZNModeBasicPID)
    {
      kpConstant = 0.6;
      tiConstant = 0.5;
      tdConstant = 0.125;
    }
    else if (handle->znMode == ZNModeLessOvershoot)
    {
      kpConstant = 0.33;
      tiConstant = 0.5;
      tdConstant = 0.33;
    }
    else
    { // Default to No Overshoot mode as it is the safest
      kpConstant = 0.2;
      tiConstant = 0.5;
      tdConstant = 0.33;
    }

    // Calculate gains
    handle->kp = kpConstant * ku;
    handle->ki = (handle->kp / (tiConstant * tu)) * handle->loopInterval;
    handle->kd = (tdConstant * handle->kp * tu) / handle->loopInterval;

    // Average all gains after the first two cycles
    if (handle->i > 1)
    {
      handle->pAverage += handle->kp;
      handle->iAverage += handle->ki;
      handle->dAverage += handle->kd;
    }

    // Reset minimum
    handle->min = handle->targetInputValue;

    // Increment cycle count
    handle->i++;
  }

  // If loop is done, disable output and calculate averages
  if (handle->i >= handle->cycles)
  {
    handle->output = false;
    handle->outputValue = handle->minOutput;
    handle->kp = handle->pAverage / (handle->i - 1);
    handle->ki = handle->iAverage / (handle->i - 1);
    handle->kd = handle->dAverage / (handle->i - 1);
  }

  return handle->outputValue;
}

// Get results of most recent tuning
float pat_zn_get_kp(pid_autotune_zn_t *handle)
{
  return handle->kp;
}

float pat_zn_get_ki(pid_autotune_zn_t *handle)
{
  return handle->ki;
}

float pat_zn_get_kd(pid_autotune_zn_t *handle)
{
  return handle->kd;
}

bool pat_zn_is_finished(pid_autotune_zn_t *handle)
{
  return (handle->i >= handle->cycles);

} // Is the tuning finished?

int pat_zn_get_cycle(pid_autotune_zn_t *handle) // return tuning cycle
{
  return handle->i;
}
